﻿#ifndef SIXJOINTROBOT_H
#define SIXJOINTROBOT_H

#define M_E        2.71828182845904523536
#define M_LOG2E    1.44269504088896340736
#define M_LOG10E   0.434294481903251827651
#define M_LN2      0.693147180559945309417
#define M_LN10     2.30258509299404568402
#define M_PI       3.14159265358979323846
#define M_PI_2     1.57079632679489661923
#define M_PI_4     0.785398163397448309616
#define M_1_PI     0.318309886183790671538
#define M_2_PI     0.636619772367581343076
#define M_2_SQRTPI 1.12837916709551257390
#define M_SQRT2    1.41421356237309504880
#define M_SQRT1_2  0.707106781186547524401

class SixJointRobot
{
public:
    SixJointRobot(void);
public:
    ~SixJointRobot(void);

private:
    float m_DH[6][4];//机器人DH参数矩阵
	float m_T60[4][4];//机器人末端目标矩阵
	float m_J[8][6];//机器人关节的8组解
	float m_bestDHJ[6];//最优解,可超过360°	
	bool m_JIsValidate[9];//8组解是否有效，最后元素表示是否有最优解
    int m_bestSolutionIndex; //最优解索引

	float A1[4][4];//连杆1的转移矩阵
	float A2[4][4];//连杆2的转移矩阵
	float A3[4][4];//连杆3的转移矩阵
	float A4[4][4];//连杆4的转移矩阵
	float A5[4][4];//连杆5的转移矩阵
	float A6[4][4];//连杆6的转移矩阵
	float A7[4][4];//连杆7的转移矩阵
	float A7LI[4][4];//连杆7的转移矩阵的逆矩阵
	float A123[4][4];//连杆1*2*3的转移矩阵
	float A456[4][4];//连杆4*5*6的转移矩阵
	float A1_6[4][4];//连杆1*2*3*4*5*6的转移矩阵
	float A1_6LI[4][4];//连杆1*2*3*4*5*6的转移矩阵的逆矩阵
	float T456[4][4];//连杆4*5*6的目标转移矩阵	
	float eox,eoy,l1,l2,bJ3;//机器人结构常数

public:
    /**
     * @brief getIkSolution 求逆解
     * @param poseMatrix　目标矩阵
     * @param beforeJoint   初始角度
     * @param jointSolution　求解角度
     * @return 　－１表示求解失败
     */
    int getIkSolution(float poseMatrix[4][4], float beforeJoint[6], float jointSolution[6]);
    /**
     * @brief getIkSolutionFUT　根据fut的配置求逆解
     * @param poseMatrix　目标位置矩阵
     * @param fut　fut数组
     * @param jointSolution　目标位置关节角
     * @return -1求解失败
     */
    int getIkSolutionFUT(float poseMatrix[4][4], int fut[6], float jointSolution[6]);
    /**
     * @brief getIkConfig 求多解参数配置，F(N),U(D),T(B)
     * @param beforeJoint 关节角输入
     * @param fut FUT参数数组,000分别代表NDB,111代表FUT
     * @return 返回负数则求解异常
     */
    int getIkConfig( float beforeJoint[6],  int fut[3]);
    /**
     * @brief initialForIk 初始化机器人参数
     * @param robotDH　ＤＨ参数
     * @return
     */
    int initialForIk(float robotDH[6][4]);

    /**
    * @brief calculatePosition 计算正解
    * @param J
    * @param poseMatrix
    * @return
    */
   int calculatePosition(float jointValue[6], float poseMatrix[][4]);

private:
    int selectBestSolution(float beforeJoint[6], float jointSolution[6]);

    int selectBestSolutionFUT(int fut[6], float jointSolution[6]);

    void calculatePosition(float DH[][4], float J[][6], int i, float P[][4]);

	void multiplymatrix(float a[][4], float b[][4],float c[][4]);

	void calculateLinkMatrix(float oi, float afi, float ai, float di, float Ai[][4]);

	void calculateLiMatrix(float a[][4], float b[][4]);

    int calculateEulerJ456(float a[][4], float J[][6],int i);

    int calculateJ1(void);

    int calculateJ2J3(void);

	void calculateEulerTransferMatrix(int i);

	void MatrixRotationXself(float Matrixa[][4], float angle);

	void MatrixRotationYself(float Matrixa[][4], float angle);

	void MatrixRotationZself(float Matrixa[][4], float angle);

	void MatrixTranslateSelfcoordinate(float Matrixa[][4],float x,float y,float z);

	void MatrixTranslateWorldcoordinate(float Matrixa[][4],float x,float y,float z);

	void MatrixEqual(float a[][4], float b[][4]);

	void MatrixEqual(float a[][4], float b[][4],int n);

	void TransferDirectJToDHJ(float DJ[6], float DHJ[6],bool F);

	void TransferDirectJToDHJ(float DJ[6], float DHJ1,float DHJ2,float DHJ3,float DHJ4,float DHJ5,float DHJ6,bool F);

	float MaxCompare(float x, float y, float z);

};

#endif
